
#include "jointcontrol.h"
#include "joints.h"
#include "robot.h"

#include <cmath>

#include <QDebug>

JointControl::JointControl(int joint_number, int joint_type, float min, float max, float start_value)
{

	jointNumber = joint_number;
	jointType = joint_type;
	minimum = double(min);
	maximum = double(max);
	currentValue = double(start_value);
	
	//scale into integer for slider control
	iSliderSteps = 1000;
	iPageStep = 100;
	iMinimum = int((iSliderSteps / (max - min)) * min - 1);
	iMaximum = int((iSliderSteps / (max - min)) * max + 1);;
	iCurrentValue = int((iSliderSteps/ (max - min)) * start_value);
	

	switch (jointType)
	{
	case REVOLUTE:
        {
                QString groupname = QString(tr("Revolute "));
	        QString number;
	        number.setNum(jointNumber);
	        groupname.append(number);

		slider = new QSlider(Qt::Horizontal);
                slider->setRange(iMinimum, iMaximum);
                slider->setSingleStep(iSliderSteps/100);
                slider->setPageStep(iPageStep);
                slider->setValue(iCurrentValue);
                slider->setTickInterval(iPageStep);
                slider->setTickPosition(QSlider::TicksAbove);

		spinbox = new QDoubleSpinBox();
                spinbox->setDecimals(2);
                spinbox->setRange(minimum, maximum);
                spinbox->setSingleStep((maximum-minimum)/500);
                spinbox->setValue(currentValue);
                spinbox->setSuffix(" deg");
		
                connect(slider, SIGNAL(valueChanged(int)), this, SLOT(sliderChanged(int)));
                connect(spinbox, SIGNAL(valueChanged(double)), this, SLOT(spinBoxChanged(double)));

		groupBox = new QGroupBox(groupname);
		groupBox->setTitle(groupname);
		hBox = new QHBoxLayout;
                hBox->addWidget(spinbox);
                hBox->addWidget(slider);
                groupBox->setLayout(hBox);

		focusBox = new QCheckBox();
		connect(focusBox, SIGNAL(clicked(bool)), this, SLOT(focusBoxClicked(bool)));
		controlLayout = new QHBoxLayout(this);
                controlLayout->addWidget(groupBox);
                controlLayout->addWidget(focusBox); 
		break;
        }
	case PRISMATIC:
	{
		QString groupname = QString(tr("Prismatic "));
	        QString number;
	        number.setNum(jointNumber);
	        groupname.append(number);

		slider = new QSlider(Qt::Horizontal);
                slider->setRange(iMinimum, iMaximum);
                slider->setSingleStep(iSliderSteps/100);
                slider->setPageStep(iPageStep);
                slider->setValue(iCurrentValue);
                slider->setTickInterval(iPageStep);
                slider->setTickPosition(QSlider::TicksAbove);

		spinbox = new QDoubleSpinBox();
                spinbox->setDecimals(2);
                spinbox->setRange(minimum, maximum);
                spinbox->setSingleStep((maximum-minimum)/100);
                spinbox->setValue(currentValue);
                spinbox->setSuffix(" m");

                connect(slider, SIGNAL(valueChanged(int)), this, SLOT(sliderChanged(int)));
                connect(spinbox, SIGNAL(valueChanged(double)), this, SLOT(spinBoxChanged(double)));

                groupBox = new QGroupBox(groupname);
		hBox = new QHBoxLayout;
                hBox->addWidget(spinbox);
                hBox->addWidget(slider);
                groupBox->setLayout(hBox);

		focusBox = new QCheckBox();
		connect(focusBox, SIGNAL(clicked(bool)), this, SLOT(focusBoxClicked(bool)));
		
                controlLayout = new QHBoxLayout(this);
                controlLayout->addWidget(groupBox);
                controlLayout->addWidget(focusBox); 
		break;
	}
	case GRIPPER:
	{
		QString groupname = QString(tr("Gripper "));
		QString number;
		number.setNum(jointNumber);
		groupname.append(number);
		
		actionLabel = new QLabel();
		checkBox = new QCheckBox();
		
		groupBox = new QGroupBox(groupname);
		hBox = new QHBoxLayout;
		hBox->addWidget(actionLabel);
		hBox->addWidget(checkBox);
		groupBox->setLayout(hBox);
		
		connect(checkBox, SIGNAL(stateChanged(int)), this, SLOT(actionStatusChanged(int)));
		actionLabel->setText("OFF");
		
		focusBox = new QCheckBox();
		connect(focusBox, SIGNAL(clicked(bool)), this, SLOT(focusBoxClicked(bool)));
		
		controlLayout = new QHBoxLayout(this);
		controlLayout->addWidget(groupBox);
		controlLayout->addWidget(focusBox);
		
		break;
	}
	default:
		qDebug()<<"EXITED CASE WITH DEFAULT";
		break;
	}
}


void JointControl::setValue(float value)	//SLOT
{
	

}


void JointControl::setActionStatus(int status) //SLOT
{


}

void JointControl::sliderChanged(int value)	//PRIVATE SLOT
{
	iCurrentValue = value;
	currentValue = double(((maximum - minimum)/iSliderSteps)*iCurrentValue);
	spinbox->setValue(currentValue);

	emit valueChanged(jointNumber, float(currentValue));
}

void JointControl::spinBoxChanged(double value)		//PRIVATE SLOT
{
	currentValue = value;
	iCurrentValue = round((iSliderSteps / ( maximum - minimum)) * currentValue);
	slider->setValue(iCurrentValue);
	
	emit valueChanged(jointNumber, float(currentValue));
}


void JointControl::actionStatusChanged(int checked)	//PRIVATE SLOT
{
	if (checked == Qt::Checked)
	{
		actionLabel->setText("ON ");
		emit valueChanged(jointNumber, 1.0);
	}
	else if (checked == Qt::Unchecked)
	{
		actionLabel->setText("OFF");
		emit valueChanged(jointNumber, 0.0);
	}
}

void JointControl::setFocus(int joint)		//SLOT
{
	
	if (joint == jointNumber)
	{
		focusBox->setChecked(true);
	}
	else
	{
		focusBox->setChecked(false);
	}
}

void JointControl::focusBoxClicked(bool checked)	//PRIVATE SLOT
{
	if (checked == true)
		emit grabFocus(jointNumber);
	else
		emit grabFocus(-1);
}
